﻿using System;
using System.Numerics;

namespace Meow.Core;

public static class Exts
{

    public enum ISqrtRoundMode { Floor, Nearest, Ceiling }
    public static int ISqrt(int number, ISqrtRoundMode round = ISqrtRoundMode.Floor)
    {
        if (number < 0)
            throw new InvalidOperationException($"Attempted to calculate the square root of a negative integer: {number}");

        return (int)ISqrt((uint)number, round);
    }

    public static uint ISqrt(uint number, ISqrtRoundMode round = ISqrtRoundMode.Floor)
    {
        var divisor = 1U << 30;

        var root = 0U;
        var remainder = number;

        // Find the highest term in the divisor
        while (divisor > number)
            divisor >>= 2;

        // Evaluate the root, two bits at a time
        while (divisor != 0)
        {
            if (root + divisor <= remainder)
            {
                remainder -= root + divisor;
                root += 2 * divisor;
            }

            root >>= 1;
            divisor >>= 2;
        }

        // Adjust for other rounding modes
        if (round == ISqrtRoundMode.Nearest && remainder > root)
            root += 1;
        else if (round == ISqrtRoundMode.Ceiling && root * root < number)
            root += 1;

        return root;
    }

    public static long ISqrt(long number, ISqrtRoundMode round = ISqrtRoundMode.Floor)
    {
        if (number < 0)
            throw new InvalidOperationException($"Attempted to calculate the square root of a negative integer: {number}");

        return (long)ISqrt((ulong)number, round);
    }

    public static ulong ISqrt(ulong number, ISqrtRoundMode round = ISqrtRoundMode.Floor)
    {
        var divisor = 1UL << 62;

        var root = 0UL;
        var remainder = number;

        // Find the highest term in the divisor
        while (divisor > number)
            divisor >>= 2;

        // Evaluate the root, two bits at a time
        while (divisor != 0)
        {
            if (root + divisor <= remainder)
            {
                remainder -= root + divisor;
                root += 2 * divisor;
            }

            root >>= 1;
            divisor >>= 2;
        }

        // Adjust for other rounding modes
        if (round == ISqrtRoundMode.Nearest && remainder > root)
            root += 1;
        else if (round == ISqrtRoundMode.Ceiling && root * root < number)
            root += 1;

        return root;
    }

    public static int MultiplyBySqrtTwo(short number)
    {
        return number * 46341 / 32768;
    }


    public static int Lerp(int a, int b, int mul, int div)
    {
        return a + (b - a) * mul / div;
    }

    public static float Lerp(float a, float b, float t)
    {
        return a + (b - a) * t;
    }

    public static int NextPowerOf2(int v)
    {
        --v;
        v |= v >> 1;
        v |= v >> 2;
        v |= v >> 4;
        v |= v >> 8;
        ++v;
        return v;
    }

    public static bool IsPowerOf2(int v)
    {
        return (v & (v - 1)) == 0;
    }

    public static int2 ToInt2(this Vector2 vector2) => new int2((int)vector2.X, (int)vector2.Y);
    public static int3 ToInt3(this Vector3 vector3) => new int3((int)vector3.X, (int)vector3.Y, (int)vector3.Z);
    public static int4 ToInt4(this Vector4 vector4) => new int4((int)vector4.X, (int)vector4.Y, (int)vector4.Z, (int)vector4.W);


}


